import robo3pi as s
import Project.Control.ControlFunctions as f
import Project.Main as Main
import time

SPEED = Main.SPEED # The speed the robot will move in M/S (Meters in second) 


class CommandClass:
    #TODO: first point angle is 0, consider calculate real angle
    def __init__(self, ID, Distance, Angle, currPos, nextPos):
        self.setID(ID)
       
        self.Distance = Distance
        self.Angle = Angle
        self.currPos = currPos
        self.nextPos = nextPos
    
    def displayCommand(self):
        print('ID = ' + self.ID + ' Distance = ' +  str(self.Distance) + ' Angle = ' + str(self.Angle))
        
    def run(self):
        if self.Angle != 0:
            f.rotateByAngle(self.getID(), self.Angle)
        if self.Distance != 0:
            print("From RUN: Dist = " + str(self.Distance))
            f.moveInLineAdapter (self.getID(), self.Distance)
        time.sleep(Main.SLEEP_TIME_COMMANDS)
        
        
    def getID(self):
        currIDNUM = int(self.ID)
        return  str(currIDNUM)
    
    def setID(self, ID):
        currIDNUM = int(ID) + Main.OFFSET_ROBOTS
        self.ID = str(currIDNUM)   
        
    def close(self):
        s.close()
